#pragma once


#include <iostream>
#include <memory>
#include <vector>
#include <queue>
#include <mutex>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/types_c.h>
#include <eigen3/Eigen/Dense>


#include <ros/ros.h>


#include <camera_modul.hpp>
#include <Frame.hpp>
#include <tool.hpp>
#include <parameter.hpp>
#include <ORBextractor.h>


#define COL 640
#define ROW 480



// 前端实现思路： 前端提取orb特征点、同时可以进行orb特征点匹配

class FeatureTracker{

private:
    std::shared_ptr<ReadParameter> mspParam;
    std::shared_ptr<EASY::ORBextractor> mspORBextractor;

    bool mEnableShowFeature = true;// 允许前端显示特征点

public:
    pinHole camera;
    bool hasPrediction = false;

    long long feature_id = 0;// 特征点的id
    int mtrackNum = 0;// 跟踪到特征点的个数
    int MaxTrackNum = 1000; //最大跟踪数量
    int MinDist = 30;//两个特征点之间最近的距离
    int BORDER_SIZE = 5;

    FeatureTracker();
    FeatureTracker(std::shared_ptr<ReadParameter> inParma);
    ~FeatureTracker();
    
    // 线程运行函数
    void run();
    // 双目计算 匹配 + 恢复深度
    void computationalStereo();
    // 
};



// copy of vins-mono
bool inBorder(const cv::Point2f &pt);
void reduceVector(std::vector<cv::Point2f> &v, std::vector<uchar> status);
void reduceVector(std::vector<int> &v, std::vector<uchar> status);
void reduceVector(std::vector<std::pair<long long, int>> &v, std::vector<uchar> status);